#include "formation_avoidance/formation.h"

gvm::GMMVoxelMapCircularBuffer gvmcb(0.1,2.5,8);
gvm::GMM gmm;


// void Formation_Agent::visual_corridor()
// {
//     visualization_msgs::MarkerArray cube_vis;

//     for(auto & mk: cube_vis.markers) 
//     mk.action = visualization_msgs::Marker::DELETE;
    
//     _corridor_vis_pub.publish(cube_vis);

//     cube_vis.markers.clear();

//     visualization_msgs::Marker mk;
//     mk.header.frame_id = "map";
//     mk.header.stamp = ros::Time::now();
//     mk.ns = "corridor";
//     mk.type = visualization_msgs::Marker::CUBE;
//     mk.action = visualization_msgs::Marker::ADD;
//     mk.lifetime = ros::Duration(PlanFreq);

//     mk.pose.orientation.x = 0.0;
//     mk.pose.orientation.y = 0.0;
//     mk.pose.orientation.z = 0.0;
//     mk.pose.orientation.w = 1.0;

//     mk.color.a = 0.4;
//     mk.color.r = 1.0;
//     mk.color.g = 1.0;
//     mk.color.b = 1.0;

//     int idx = 10;
//     double _is_proj_cube=false;
//     for(int i = 0; i < int(CorridorCube.size()); i++)
//     {   
//         mk.color.r = i/CorridorCube.size();
//         mk.id = idx;

//         mk.pose.position.x = (CorridorCube[i].vertex_pos(0, 0) + CorridorCube[i].vertex_pos(3, 0) ) / 2.0; 
//         mk.pose.position.y = (CorridorCube[i].vertex_pos(0, 1) + CorridorCube[i].vertex_pos(1, 1) ) / 2.0; 

//         if(_is_proj_cube)
//             mk.pose.position.z = 0.0; 
//         else
//             mk.pose.position.z = (CorridorCube[i].vertex_pos(0, 2) + CorridorCube[i].vertex_pos(4, 2) ) / 2.0; 

//         mk.scale.x = abs(CorridorCube[i].vertex_pos(0, 0) - CorridorCube[i].vertex_pos(3, 0) );
//         mk.scale.y = abs(CorridorCube[i].vertex_pos(1, 1) - CorridorCube[i].vertex_pos(0, 1) );

//         if(_is_proj_cube)
//             mk.scale.z = 0.05; 
//         else
//             mk.scale.z = abs(CorridorCube[i].vertex_pos(0, 2) - CorridorCube[i].vertex_pos(4, 2) );

//         idx ++;
//         cube_vis.markers.push_back(mk);
//     }

//     _corridor_vis_pub.publish(cube_vis);
// }


void Formation_Agent::init(ros::NodeHandle &nh)
{
    ros::Duration(2.0).sleep();
    cloud_sub=nh.subscribe("/pcl_render_node_cloud", 1, &Formation_Agent::cloudCallback,this);
    // cloud_sub=nh.subscribe("/iris/camera/depth/points", 1, &Formation_Agent::cloudCallback,this);
    init_path_visual_pub = nh.advertise<visualization_msgs::Marker>("/init_path_visual", 1);
	pose_sub=nh.subscribe("/visual_slam/odom", 1, &Formation_Agent::poseCallback,this);
    optimize_traj_visual_pub = nh.advertise<visualization_msgs::Marker>("/optimize_traj_visual", 1);
    _corridor_vis_pub = nh.advertise<visualization_msgs::MarkerArray>("/corridor", 1);
    exec_timer_ = nh.createTimer(ros::Duration(FSMFreq), &Formation_Agent::execFSMCallback, this);
    optimize_traj_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/optimize_traj", 1);
    BsplineTraj_pub = nh.advertise<formation_avoidance::OptimizeTraj>("/BsplineTraj", 1);
    plan_head_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/planHeadAngle", 1);
    gimbal_angle_sub=nh.subscribe("/iris/gimbal_yaw_angle", 1, &Formation_Agent::gimbal_yaw_angleCallback,this);
    gvmcb_map_pub= nh.advertise<sensor_msgs::PointCloud2>("/gvmcb_map", 1, true);
	cloud_edf_pub= nh.advertise<sensor_msgs::PointCloud2>("/edtMap_visual", 1, true);
	trans_cloud_pub= nh.advertise<sensor_msgs::PointCloud2>("/trans_cloud", 1, true);
    state_sub=nh.subscribe("/mavros/state", 1, &Formation_Agent::state_cb,this);
	other_gmm_sub =  nh.subscribe("/gmm_server", 6, &Formation_Agent::gmmCallback,this);
    gmm_pub = nh.advertise<gmm_map::GMMCommunication>("/gmm_server",1,true) ;
    vel_pub = nh.advertise<geometry_msgs::PoseStamped>("/agent_velocity",1,true) ;
	map_pub_timer_=nh.createTimer(ros::Duration(MapUpdateFreq), &Formation_Agent::map_pubCallback, this);


	for(int i=0;i<AGENT_NUM;i++)
	{
		std::ostringstream str_iris;
		str_iris<<"/iris_"<<i;
		agent_head_sub[i]=nh.subscribe(str_iris.str()+"/gimbal_yaw_angle", 1, &Formation_Agent::gimbal_yaw_angleCallback,this);
	}

	other_global_position_sub1= nh.subscribe("/other1_visual_slam/odom", 1, &Formation_Agent::other_global_position_cb1,this);
	other_global_position_sub2= nh.subscribe("/other2_visual_slam/odom", 1, &Formation_Agent::other_global_position_cb2,this);
	other_global_position_sub3= nh.subscribe("/other3_visual_slam/odom", 1, &Formation_Agent::other_global_position_cb3,this);

	init_head_yaw=0;
	CLOUD_DELAY=0;

    D435_T265_OFFSET_X=0.1;     //D435在T265前方为正
    D435_T265_OFFSET_Z=0.0;     //D435在T265上方为正

    D435_BASE_OFFSET_X=0.0;     //D435在云台基座前方为正
    D435_BASE_OFFSET_Z=0.0;    //D435在云台基座上方为正

    T265_OFFSET_X=0.0;        //T265在无人机中心前方为正
    T265_OFFSET_Y=0.0;        //T265在无人机中心左边为正
    T265_OFFSET_Z=0.0;        //#T265在无人机中心上方为正

    CLIP_DISTANCE=0.01;         //点云截断距离
    map_initialized=false;

    for(int i=0;i<AGENT_NUM;i++)
    {
        current_p[i]=Eigen::Vector3d::Zero();
		current_p[i](2)=1.8;
    }

    plan_end_p=Eigen::Vector3d::Zero();
    cloud_filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
	cloud_clip.reset(new pcl::PointCloud<pcl::PointXYZ>());
    distributed_optimizer.init(nh);

    AgentInitFlag=true;
    ROS_INFO("AGENT id:%d INIT---",AGENT_ID);

    local_goal<<0,0,1.5;
    exec_state_=INIT;

	gmm.setAgentId(AGENT_ID,COLOR_ID);

	currentHeadAngle=Eigen::VectorXd::Zero(AGENT_NUM);


	ROS_INFO("AGENT id:%d ppppppppppppppppppppppppppp---",AGENT_ID);


	formation_trajectory_optimizer.LinkMap(&gvmcb,OPTIMIZE_SAFETY_DISTANCE,v,max_acc,AGENT_ID,rate);

	astar_searcher.AstarLinkMap(&gvmcb,local_length,AGENT_ID);

	PathExchanger.init(nh,AGENT_ID,AGENT_NUM);


	f = boost::bind(&Formation_Agent::callback,this, _1, _2);
	srv.setCallback(f);


	
}


void Formation_Agent::callback(formation_avoidance::MyStuffConfig &config, uint32_t level )
{
	ROS_ERROR("ID : %d change lambda ---------------",AGENT_ID);


	formation_trajectory_optimizer.lamda_Smoothness=config.lamda_Smoothness;
	formation_trajectory_optimizer.lamda_Distance=config.lamda_Distance;
	formation_trajectory_optimizer.lamda_Feasibility=config.lamda_Feasibility;
	formation_trajectory_optimizer.lamda_Collision=config.lamda_Collision;
	formation_trajectory_optimizer.lamda_HeightLimitation=config.lamda_HeightLimitation;
	formation_trajectory_optimizer.lamda_EndPoint=config.lamda_EndPoint;
	formation_trajectory_optimizer.lamda_velocityDirectionCost=config.lamda_velocityDirectionCost;
	formation_trajectory_optimizer.lamda_SecondHalfCollisionPunishRatio=config.lamda_SecondHalfCollisionPunishRatio;
	formation_trajectory_optimizer.lamda_Formation=config.lamda_Formation;

	formation_trajectory_optimizer.lamda_BaseFormation=config.lamda_BaseFormation;


 }


void Formation_Agent::state_cb(const mavros_msgs::State::ConstPtr& msg){
	 current_state = *msg;
}

void Formation_Agent::angle_mapping(double& angle)
{
    while(angle>=PI)
        angle-=2*PI;
    while(angle<-PI)
        angle+=2*PI;
}
void Formation_Agent::gimbal_yaw_angleCallback(geometry_msgs::PoseStamped msg)
{
    angle_mapping(msg.pose.position.x);

	for(int i=0;i<AGENT_NUM;i++)
	{
		std::ostringstream str_iris;
		str_iris<<"iris_"<<i;
		if(msg.header.frame_id==str_iris.str())
		{
			currentHeadAngle(i)=msg.pose.position.x;
			if(i==AGENT_ID)
			{
				motor_yaw_queue.push(msg);
				if(motor_yaw_queue.size()>50)
					motor_yaw_queue.pop();
			}
			break;
		}
	}
}

void Formation_Agent::poseCallback(nav_msgs::Odometry msg)
{
	static double last_time=ros::Time::now().toSec();
    current_p[AGENT_ID](0) = msg.pose.pose.position.x;
    current_p[AGENT_ID](1) = msg.pose.pose.position.y;
    current_p[AGENT_ID](2) = msg.pose.pose.position.z;

    current_q[AGENT_ID](0) = msg.pose.pose.orientation.x;
    current_q[AGENT_ID](1) = msg.pose.pose.orientation.y;   
    current_q[AGENT_ID](2) = msg.pose.pose.orientation.z;
    current_q[AGENT_ID](3) = msg.pose.pose.orientation.w;


	current_v[AGENT_ID]=(current_p[AGENT_ID]-last_current_p[AGENT_ID])/(msg.header.stamp.toSec()-last_time);
	last_time=msg.header.stamp.toSec();
	last_current_p[AGENT_ID]=current_p[AGENT_ID];
	
	geometry_msgs::PoseStamped velocity_msg;
	velocity_msg.header.stamp=msg.header.stamp;
	velocity_msg.pose.position.x=current_v[AGENT_ID](0);
	velocity_msg.pose.position.y=current_v[AGENT_ID](1);
	velocity_msg.pose.position.z=current_v[AGENT_ID](2);

	vel_pub.publish(velocity_msg);

	current_pose_time=msg.header.stamp.toSec();

	if(!gvmInitial)
	{
		gvmcb.setMapInitPose(current_p[AGENT_ID]);
		gvmInitial=true;
	}
	gvmcb.updateOrigin(current_p[AGENT_ID]);
	getPositionFlag=getPositionFlag|1;

	std::string str_iris;
	str_iris="/uav_"+std::to_string(AGENT_ID)+"/base";

	static tf2_ros::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = str_iris;
    tfs.transform.translation.x =msg.pose.pose.position.x;
    tfs.transform.translation.y = msg.pose.pose.position.y;
    tfs.transform.translation.z = msg.pose.pose.position.z;
    tfs.transform.rotation.x = 0;
    tfs.transform.rotation.y = 0;
    tfs.transform.rotation.z = 0;
    tfs.transform.rotation.w =1;

    broadcaster.sendTransform(tfs);

}


void Formation_Agent::other_global_position_cb1(nav_msgs::Odometry msg)
{
	static double last_time=ros::Time::now().toSec();

	current_p[other_id1](0)=msg.pose.pose.position.x;
	current_p[other_id1](1)=msg.pose.pose.position.y;
	current_p[other_id1](2)=msg.pose.pose.position.z;

	current_v[other_id1](0) = msg.twist.twist.linear.x;
	current_v[other_id1](1) = msg.twist.twist.linear.y;
	current_v[other_id1](2) = msg.twist.twist.linear.z;

	current_v[other_id1]=(current_p[other_id1]-last_current_p[other_id1])/(msg.header.stamp.toSec()-last_time);
	last_time=msg.header.stamp.toSec();
	last_current_p[other_id1]=current_p[other_id1];

	getPositionFlag=getPositionFlag|2;

}

void Formation_Agent::other_global_position_cb2(nav_msgs::Odometry msg)
{
	static double last_time=ros::Time::now().toSec();

	current_p[other_id2](0)=msg.pose.pose.position.x;
	current_p[other_id2](1)=msg.pose.pose.position.y;
	current_p[other_id2](2)=msg.pose.pose.position.z;

	current_v[other_id2](0) = msg.twist.twist.linear.x;
	current_v[other_id2](1) = msg.twist.twist.linear.y;
	current_v[other_id2](2) = msg.twist.twist.linear.z;

	current_v[other_id2]=(current_p[other_id2]-last_current_p[other_id2])/(msg.header.stamp.toSec()-last_time);
	last_time=msg.header.stamp.toSec();
	last_current_p[other_id2]=current_p[other_id2];

	getPositionFlag=getPositionFlag|4;

}
void Formation_Agent::other_global_position_cb3(nav_msgs::Odometry msg)
{
	static double last_time=ros::Time::now().toSec();

	current_p[other_id3](0)=msg.pose.pose.position.x;
	current_p[other_id3](1)=msg.pose.pose.position.y;
	current_p[other_id3](2)=msg.pose.pose.position.z;

	current_v[other_id3](0) = msg.twist.twist.linear.x;
	current_v[other_id3](1) = msg.twist.twist.linear.y;
	current_v[other_id3](2) = msg.twist.twist.linear.z;

	current_v[other_id3]=(current_p[other_id3]-last_current_p[other_id3])/(msg.header.stamp.toSec()-last_time);
	last_time=msg.header.stamp.toSec();
	last_current_p[other_id3]=current_p[other_id3];	
	
	getPositionFlag=getPositionFlag|8;
	// ROS_INFO("getPositionFlag: %d",getPositionFlag);

}

void Formation_Agent::map_pubCallback(const ros::TimerEvent &e)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr gvmcbMap_pc(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_edf_field(new pcl::PointCloud<pcl::PointXYZRGB>());

	int visual_map_range=50;

	for(int i=gvmcb.indexRange()/2-15;i<gvmcb.indexRange()/2+15;i++)
	{
		for(int j=gvmcb.indexRange()/2-visual_map_range;j<gvmcb.indexRange()/2+visual_map_range;j++)
		{
			for(int k=gvmcb.indexRange()/2-visual_map_range;k<gvmcb.indexRange()/2+visual_map_range;k++)
			{
				Eigen::Vector3i idx(k,j,i);
				if(gvmcb.getMapLocalIdxOccupied(idx))
				{
					Eigen::Vector3d tmp=gvmcb.getMapGlobalPosfromLocalIdx(idx);
					// ROS_INFO("idx: %d %d %d",idx(0),idx(1),idx(2));
					// ROS_INFO("tmp %f %f %f",tmp(0),tmp(1),tmp(2));
					pcl::PointXYZ vis_point;
					vis_point.x = tmp(0);
					vis_point.y = tmp(1);
					vis_point.z = tmp(2);
					gvmcbMap_pc->points.push_back(vis_point);
				}
				if(i==gvmcb.indexRange()/2)
				{
					Eigen::Vector3d tmp=gvmcb.getMapGlobalPosfromLocalIdx(idx);
					pcl::PointXYZRGB vis_point;
					vis_point.x = tmp(0);
					vis_point.y = tmp(1);
					vis_point.z = tmp(2);
					Eigen::Vector3i point_RGB=gvmcb.getLocalIdxMapEdtColor(idx);
					vis_point.r = point_RGB(0);
					vis_point.g = point_RGB(1);
					vis_point.b = point_RGB(2);
					cloud_edf_field->points.push_back(vis_point);
				}
			}
		}
	}

	sensor_msgs::PointCloud2 gvmcbMap_ros;
	pcl::toROSMsg(*gvmcbMap_pc, gvmcbMap_ros);
	gvmcbMap_ros.header.stamp = ros::Time::now();
	gvmcbMap_ros.header.frame_id = "map";
	gvmcb_map_pub.publish(gvmcbMap_ros);

	sensor_msgs::PointCloud2 edf_ros_cloud;
	pcl::toROSMsg(*cloud_edf_field, edf_ros_cloud);
	edf_ros_cloud.header.stamp = ros::Time::now();
	edf_ros_cloud.header.frame_id = "map";
	cloud_edf_pub.publish(edf_ros_cloud);

}



void Formation_Agent::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
	// ROS_INFO("ID: %d PathExchangerLock: %d gvmInitial: %d getPositionFlag: %d",AGENT_ID,PathExchangerLock,gvmInitial,getPositionFlag);
	if(PathExchangerLock)return;
	if(!gvmInitial) return;
	if(getPositionFlag!=15)return;
	ros::Time cloudcbstart=ros::Time::now();

    Eigen::Matrix4d transform;
	// transform = vehicle2ground*d435base2vehicle*d4352d435base;  //d435 to ground
	transform=Eigen::Matrix4d::Identity();
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*cloud, *cloud_in);
    if(cloud_in->points.size()<=20) return;

	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_in);
	static float res = 0.2;
	sor.setLeafSize(res, res, res);
	sor.filter(*cloud_filtered);

	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud (cloud_filtered);
	pass.setFilterFieldName ("z");// 滤波字段设置为z轴方向
	pass.setFilterLimits (-1.0, 1.5);
	// ROS_INFO("CLIP_DISTANCE: %f",CLIP_DISTANCE);
	pass.setFilterLimitsNegative (false); //设置保留范围内还是过滤掉范围内, 默认为flase，可以注释掉。true为过滤掉范围内的，flase为过滤掉范围外的
	pass.filter (*cloud_2);

	// ROS_INFO("\033[1;%dm  cloud_2 size: %ld \033[0m",COLOR_ID,cloud_2->points.size());

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudAtedge(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudNotAtedge(new pcl::PointCloud<pcl::PointXYZRGB>());

	int n_pointAtedge=0;


	for(int i=0;i<(int)cloud_2->points.size();i++)
	{
		Eigen::Vector3d tmp_point(cloud_2->points[i].x,cloud_2->points[i].y,cloud_2->points[i].z);
		// if(pointBelongToOtherAgents(tmp_point))
		// 	continue;
		pcl::PointXYZRGB point;
		point.x=cloud_2->points[i].x;
		point.y=cloud_2->points[i].y;
		point.z=cloud_2->points[i].z;
		point.b=255;
		cloudNotAtedge->points.push_back(point);
	}
	// ros::Time tmp_start=ros::Time::now();
	if(cloudNotAtedge->points.size()<5)return;
	/// 创建kd树
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree->setInputCloud(cloudNotAtedge);

	/// 设置分割参数
	vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
	ec.setClusterTolerance(0.3);	//设置近邻搜索的半径
	ec.setMinClusterSize(1);		//设置最小聚类点数
	ec.setMaxClusterSize(999999);	//设置最大聚类点数
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloudNotAtedge);
	ec.extract(cluster_indices);

	/// 执行欧式聚类分割，并保存分割结果
	std::vector<Eigen::Vector3d> initial_means;
	std::vector<Eigen::Vector3d> means;
    std::vector<Eigen::Matrix3d> var;
    std::vector<double> pi_vector;
	if(cluster_indices.size()==0)return;
	for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		Eigen::Vector3d cluster_centre=Eigen::Vector3d::Zero();
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
		double num=0;
		for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
		{
			Eigen::Vector3d	tmp(cloudNotAtedge->points[*pit].x,cloudNotAtedge->points[*pit].y,cloudNotAtedge->points[*pit].z);
			cluster_centre+=tmp;
			num++;
		}
		cluster_centre/=(double)num;
		// ROS_INFO("cluster_centre: %f %f %f",cluster_centre(0),cluster_centre(1),cluster_centre(2));
		initial_means.push_back(cluster_centre);

		pi_vector.push_back((double)num/(double)cloudNotAtedge->points.size());

		Eigen::Matrix3d var_tmp=Eigen::Matrix3d::Zero();

		for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
		{
			Eigen::Vector3d	tmp(cloudNotAtedge->points[*pit].x,cloudNotAtedge->points[*pit].y,cloudNotAtedge->points[*pit].z);
			var_tmp+=(tmp-cluster_centre)*(tmp-cluster_centre).transpose();
		}		
		var_tmp/=(double)num;
		var.push_back(var_tmp);

	}
	means=initial_means;
	// ros::Time tmp_end=ros::Time::now();
	// ROS_INFO("ECULIDIAN USE TIME: %f",(tmp_end-tmp_start).toSec());

	gvmcb.updateOrigin(current_p[AGENT_ID]);
	//fit points not at edge
	gmm.init(pi_vector,means,var,means.size(),current_p[AGENT_ID]);
	// ros::Time tmp_start2=ros::Time::now();
	gvmcb.MCRayTracing(gmm,false,1.6);
	// ros::Time tmp_end2=ros::Time::now();
	// ROS_INFO("RAY TRACING USE: %f",(tmp_end2-tmp_start2).toSec());
	gvmcb.updateEDTMap();
	sendGmmMsg(gmm);
	ros::Time tmp2=ros::Time::now();
	// ROS_INFO_THROTTLE(1,"GMM USE TIME: %f",(tmp2-tmp1).toSec());
	means.clear();
	var.clear();
	pi_vector.clear();


    pcl::PointCloud<pcl::PointXYZ>::Ptr trans_cloud(new pcl::PointCloud<pcl::PointXYZ>());
	
    for(int i = 0; i <(int) cloudNotAtedge->points.size(); ++i)
    {
        pcl::PointXYZ vis_point;

        vis_point.x = cloudNotAtedge->points.at(i).x;
        vis_point.y = cloudNotAtedge->points.at(i).y;
        vis_point.z = cloudNotAtedge->points.at(i).z;

        trans_cloud->points.push_back(vis_point);

    }

    sensor_msgs::PointCloud2 trans_cloud_ros;
    pcl::toROSMsg(*trans_cloud, trans_cloud_ros);
    trans_cloud_ros.header.stamp = ros::Time::now();
    trans_cloud_ros.header.frame_id = "map";
    trans_cloud_pub.publish(trans_cloud_ros);


	ros::Time cloudcbend=ros::Time::now();

    ROS_INFO_THROTTLE(5,"\033[1;%dm CLOUD EXIT, cloudCB use time: %f ======== \033[0m",COLOR_ID,(cloudcbend-cloudcbstart).toSec());

}




void Formation_Agent::sendGmmMsg(gvm::GMM self_gmm)
{
	gmm_map::GMMCommunication gmm_msg;
	gmm_msg.agent_id=AGENT_ID;
	gmm_msg.gmm_number=self_gmm.n_component_;
	
	for(int i=0;i<self_gmm.n_component_;i++)
	{
		gmm_msg.pi.push_back(self_gmm.pi_vector_[i]);

		gmm_msg.means.push_back(self_gmm.means_[i](0));
		gmm_msg.means.push_back(self_gmm.means_[i](1));
		gmm_msg.means.push_back(self_gmm.means_[i](2));

		for(int j=0;j<3;j++)
		{
			for(int k=0;k<3;k++)
			{
				gmm_msg.vars.push_back(self_gmm.var_[i](j,k));
			}
		}
	}

	gmm_msg.sensor_location_x=self_gmm.sensor_location_(0);
	gmm_msg.sensor_location_y=self_gmm.sensor_location_(1);
	gmm_msg.sensor_location_z=self_gmm.sensor_location_(2);

	gmm_msg.header.stamp=ros::Time::now();
	gmm_pub.publish(gmm_msg);

}

void Formation_Agent::gmmCallback( const gmm_map::GMMCommunication& gmm_msg)
{
	if(PathExchangerLock)return;
	if(!gvmInitial)return;
	if(gmm_msg.agent_id==AGENT_ID)
		return;
	map_initialized=true;
	// ROS_INFO("\033[1;%dm id:%d GET Gmm from id:%d gmm number: %d!!!!!!!!!!! \033[0m",COLOR_ID,AGENT_ID,gmm_msg.agent_id,gmm_msg.gmm_number);

	// ros::Time starttime=ros::Time::now();

	int gmm_number=gmm_msg.gmm_number;
    std::vector<Eigen::Vector3d> means;
    std::vector<Eigen::Matrix3d> var;
    std::vector<double> pi_vector;

	for(int i=0;i<gmm_number;i++)
	{
		Eigen::Vector3d tmp;
		tmp<<gmm_msg.means[i*3+0],gmm_msg.means[i*3+1],gmm_msg.means[i*3+2];
		means.push_back(tmp);

		Eigen::Matrix3d tmp_var;
		tmp_var<<gmm_msg.vars[i*9+0],gmm_msg.vars[i*9+1],gmm_msg.vars[i*9+2],
				gmm_msg.vars[i*9+3],gmm_msg.vars[i*9+4],gmm_msg.vars[i*9+5],
				gmm_msg.vars[i*9+6],gmm_msg.vars[i*9+7],gmm_msg.vars[i*9+8];
		var.push_back(tmp_var);

		double tmp_pi=gmm_msg.pi[i];
		pi_vector.push_back(tmp_pi);
	}

	Eigen::Vector3d sensor_location(gmm_msg.sensor_location_x,gmm_msg.sensor_location_y,gmm_msg.sensor_location_z);
	
	gvm::GMM other_gmm;

	other_gmm.init(pi_vector,means,var,means.size(),sensor_location);

	gvmcb.updateOrigin(current_p[AGENT_ID]);
	gvmcb.MCRayTracing(other_gmm,false,1.6);
	gvmcb.updateEDTMap();

	// ros::Time endtime=ros::Time::now();

	// ROS_INFO("\033[1;%dm  gmmCallback use time: %f\033[0m",COLOR_ID,(endtime-starttime).toSec());
}



bool Formation_Agent::traj_pub()
{

	formation_avoidance::OptimizeTraj  msg;
	msg.traj_start_time=ros::Time(sendBsplineTraj[AGENT_ID].start_time);
	msg.duration_time=sendBsplineTraj[AGENT_ID].duration_time;
	for(int i=0;i<sendBsplineTraj[AGENT_ID].control_points_.rows();i++)
	{
		geometry_msgs::Point pos_pts;
		pos_pts.x=sendBsplineTraj[AGENT_ID].control_points_(i,0);
		pos_pts.y=sendBsplineTraj[AGENT_ID].control_points_(i,1);
		pos_pts.z=sendBsplineTraj[AGENT_ID].control_points_(i,2);
		msg.control_points.push_back(pos_pts);
	}

	for(int i=0;i<sendBsplineTraj[AGENT_ID].u_.size();i++)
	{
		msg.knots.push_back(sendBsplineTraj[AGENT_ID].u_(i));
	}

	for(int i=0;i<planHeadAngleKnotVector.size();i++)
	{
		msg.head_angle_knot_time.push_back(ros::Time(planHeadAngleKnotVector[i].first));
		msg.head_angle.push_back(planHeadAngleKnotVector[i].second(AGENT_ID));

	}


	BsplineTraj_pub.publish(msg);

	//for taj visual
    traj_points.id = 909+AGENT_ID*2;
    traj_points.header.frame_id = "map";
    traj_points.header.stamp = ros::Time::now();
    traj_points.type = visualization_msgs::Marker::SPHERE_LIST;
    traj_points.action = visualization_msgs::Marker::ADD;
    traj_points.ns = "lines_and_points";
    traj_points.scale.x = 0.1;
    traj_points.scale.y = 0.1;
    traj_points.scale.z = 0.1;
    traj_points.lifetime = ros::Duration(0);
	traj_points.points.clear();
	std_msgs::ColorRGBA color;
	switch (AGENT_ID)
	{
		case 0:
			color.r=1.0;color.g=0;color.b=0.0;color.a=1;
		break;
		case 1:
			color.r=1.0;color.g=1;color.b=0.0;color.a=1;
		break;
		case 2:
			color.r=0.0;color.g=0;color.b=1.0;color.a=1;
		break;
		case 3:
			color.r=0.5;color.g=0.4;color.b=0.03;color.a=1;
		break;
	}
	traj_points.color=color;
	for(int id=0;id<AGENT_NUM;id++)
	{
		for(int i=0;;i++)
		{
			double t_cur=i*0.05;
			if(t_cur>sendBsplineTraj[id].duration_time)break;
			Eigen::Vector3d pos = sendBsplineTraj[id].evaluateDeBoorT(t_cur);
			geometry_msgs::Point point;		
			point.x = pos(0);
			point.y = pos(1);
			point.z = pos(2);
			traj_points.points.push_back(point);
		}
	}
    optimize_traj_visual_pub.publish(traj_points);

    return true;
}

double Formation_Agent::getPathIG(std::vector<Eigen::Vector3i> raycastpath)
{
    double pathIG=0;
    // double uu=0;

    for (int i=0;i<(int) raycastpath.size();i+=3 )
    {
        if(i >=(int) raycastpath.size())break;
        if( gvmcb.getMapLocalIdxOccupied(raycastpath[i])) break;

		pathIG+=gvmcb.getMapLocalIdxEntropy(raycastpath[i]);
    }
    // ROS_INFO("pathIG:  %f uu:  %f",pathIG,uu);

    return std::min(pathIG,35.0);

}

double Formation_Agent::GetIG(double theta, Eigen::Vector3d observe_p)
{
    // std::cout<<"theta "<<theta<<std::endl;
    double IG=0;

    if(theta>PI)
    {
        theta=PIx2-theta;
    }

    int angle_offset=50;
    int left_sector_index=(theta+hfov/2)/SectorAngle+1+angle_offset;
    int right_sector_index=(theta-hfov/2)/SectorAngle+angle_offset;

    // ROS_INFO("left_sector_index %d right_sector_index %d",left_sector_index,right_sector_index);

	Eigen::Vector3i camera_index;
    gvmcb.getmapIdxLocal(observe_p,camera_index);

    Eigen::Vector3d raycast_p;


    for(int sector_index=right_sector_index;sector_index<=left_sector_index;sector_index++)
    {
        double sector_angle=(sector_index-angle_offset)*SectorAngle;

        if(IGSector[sector_index][1]!=-1)
        {
            // ROS_INFO("REPEAT ANGLE : %f",(sector_index-angle_offset)*SectorAngle);
            // ROS_INFO("REPEAT CALCULATION!!!!!!!!!!!!");
            // ROS_INFO("angle : %f pathig: %f",sector_angle,IGSector[sector_index][1]);

            continue;
        }
        IGSector[sector_index][1]=0;
        Eigen::Matrix3d trans;
        trans<<cos(sector_angle),-sin(sector_angle),0,
               sin(sector_angle),cos(sector_angle),0,
               0,0,1;
		// ROS_INFO("camera_node index: %d %d %d vmax: %f",camera_node.index(0),camera_node.index(1),camera_node.index(2),vmax);
        for(int v_index=-1;v_index<=1;v_index+=2)
        {
            Eigen::Vector3d corner(MaxCameraDis,0,vmax/4.0*v_index);
            raycast_p=observe_p+trans*corner;
            Eigen::Vector3i raycast_index;
            gvmcb.getmapIdxLocal(raycast_p,raycast_index);
            std::vector<Eigen::Vector3i> raycastpath=getbresenham3D(camera_index,raycast_index);
			// ROS_INFO("raycast_node index: %d %d %d",raycast_node.index(0),raycast_node.index(1),raycast_node.index(2));
			// ROS_INFO("raycastpath size: %ld ,sector_angle: %f ",raycastpath.size(),sector_angle);
            // IG+=getPathIG(raycastpath);
            IGSector[sector_index][1]+=getPathIG(raycastpath);
        }
        // ROS_INFO("angle : %f pathig: %f",sector_angle,IGSector[sector_index][1]);
        
    }
    for(int sector_index=right_sector_index;sector_index<=left_sector_index-1;sector_index++)
    {
        double sector_angle=(sector_index-angle_offset)*SectorAngle;

        if(sector_index==right_sector_index)
        {   
            double beta=(theta-hfov/2)-sector_angle;
            double b=IGSector[sector_index+1][1];
            double a=IGSector[sector_index][1];
            IG+=((b-a)*beta/SectorAngle+a+b)*(SectorAngle-beta)/2.0;
        }
        else if(sector_index==left_sector_index-1)
        {
            double beta=(theta+hfov/2)-sector_angle;
            double b=IGSector[sector_index+1][1];
            double a=IGSector[sector_index][1];

            IG+=((b-a)*beta/SectorAngle+a+a)*beta/2.0;

        }
        else
            IG+=fabs(IGSector[sector_index+1][1]+IGSector[sector_index][1])*SectorAngle/2;
    }

    return IG;
}


double Formation_Agent::GetCG(double theta1,double theta2,Eigen::Vector3d observe_p1,Eigen::Vector3d observe_p2)
{
    double CG=0;

    Eigen::Matrix3d trans_theta1;
    trans_theta1<<cos(theta1),-sin(theta1),0,
        sin(theta1),cos(theta1),0,
        0,0,1;

    Eigen::Matrix3d trans_theta2;
    trans_theta2<<cos(theta2),-sin(theta2),0,
        sin(theta2),cos(theta2),0,
        0,0,1;

    Eigen::Vector2f triangle1[3];
    triangle1[0]<<observe_p1(0),observe_p1(1);

    for(int i=1;i<=2;i++)
    {
        Eigen::Vector3d corner(MaxCameraDis,hmax/2.0*pow(-1,i+1),0);
        Eigen::Vector3d vector1=observe_p1+trans_theta1*corner;
        triangle1[i]<<vector1(0),vector1(1);
    }
    Eigen::Vector2f triangle2[3];
    triangle2[0]<<observe_p2(0),observe_p2(1);

    for(int i=1;i<=2;i++)
    {
        Eigen::Vector3d corner(MaxCameraDis,hmax/2.0*pow(-1,i+1),0);
        Eigen::Vector3d vector2=observe_p2+trans_theta2*corner;
        triangle2[i]<<vector2(0),vector2(1);
    }

    // CG=0.5*hmax*MaxCameraDis-getIntersectionTwoTriangles(triangle1,triangle2);
    CG=getIntersectionTwoTriangles(triangle1,triangle2);

    // ROS_INFO("\033[1;%dm agent id: %d  theta1: %f theta2: %f FovCG: %f\033[0m",COLOR_ID,AGENT_ID,theta1,theta2,CG);

    return CG;
}


void Formation_Agent::HeadPlanning()
{   
    ros::Time start_time=ros::Time::now();

    ROS_WARN("START HEAD PLANNING----------");

    Eigen::Vector3d observe_position(1,-1,2.5);

    // double theta=0.1;    
    for(int i=0;i<100;i++)IGSector[i][1]=-1;

    Eigen::VectorXd last_head(AGENT_NUM);
    last_head<<-3.0,-1.57,0,1.57;


    for(int i=0;i<AGENT_NUM;i++)
    {
        for(int j=0;j<100;j++)IGSector[j][1]=-1;
        double ig=GetIG(last_head(i),observe_position);
        ROS_INFO("THETA %f ig: %f",last_head(i),ig);
    }

    // double theta1=0.1; 
    // double theta2=0;
    // Eigen::Vector3d observe_p1(0,0,2.5); 
    // Eigen::Vector3d observe_p2(0,0,2.5);

    // ROS_INFO("FFFFFFFFFFFFFFFFFFFFFFFFFF");
    // GetCG(theta1,theta2,observe_p1,observe_p2);

    // theta1=0.0; 
    // theta2=0.1;

    // GetCG(theta1,theta2,observe_p1,observe_p2);
    // ROS_INFO("FFFFFFFFFFFFFFFFFFFFFFFFFF");

    // GetCG(theta1,theta2,observe_p1,observe_p2);
    // GetCG(theta1,theta2,observe_p1,observe_p2);

    ros::Time end_time=ros::Time::now();

    ROS_INFO("IG SPEND TIME: %f",(end_time-start_time).toSec());

}

double Formation_Agent::getHeadCost(Eigen::VectorXd headvector, bool base_value,double compare)
{

    double J1=0;
    double J2=0;
    double J3=0;
	double J4=0;
    double cost=0;

	for(int id=0;id<AGENT_NUM;id++)
		J1+=GetIG(headvector(id),headcost_p[id]);

	for(int i=0;i<AGENT_NUM;i++)
		for(int j=i;j<AGENT_NUM;j++)
			J2+=GetCG(headvector(i),headvector(j),headcost_p[i],headcost_p[j]);

	J3=(last_head-headvector).squaredNorm();

	J4=(velocity_angle-headvector).squaredNorm();

	cost=K_IG*J1+K_CG*J2+K_LAST*J3+K_V*J4;

    return cost;

}


void Formation_Agent::DistributeOptimizingHeadAngle()
{

    distributed_optimizer.RegisterCallBack(std::bind(&Formation_Agent::getHeadCost, this, std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));

	double traj_duration=sendBsplineTraj[0].duration_time;
	double traj_start_time=sendBsplineTraj[0].start_time;

	if(first_distributed_head)
	{
		for(int i=0;i<planHeadAngleKnotVector.size();i++)
		{
			double t_cur=sendBsplineTraj[0].duration_time/(planHeadAngleKnotVector.size()-1)*i;
			planHeadAngleKnotVector[i].first=traj_start_time+t_cur;
			planHeadAngleKnotVector[i].second<<0,0,-1,1;
		}
		first_distributed_head=false;
		return;
	}

    velocity_angle= Eigen::VectorXd::Zero(AGENT_NUM);

	std::vector< std::pair<double,Eigen::VectorXd>> last_planHeadAngleKnotVector=planHeadAngleKnotVector;

	for(int i=0;i<planHeadAngleKnotVector.size();i++)
	{
		if(i==0)
		{
			planHeadAngleKnotVector[i].first=traj_start_time;

			if(traj_start_time>=last_planHeadAngleKnotVector.back().first)
			{
				planHeadAngleKnotVector[i].second=last_planHeadAngleKnotVector.back().second;
			}
			else{

				for(int j=0;j<last_planHeadAngleKnotVector.size()-1;j++)
				{
					if(traj_start_time>=last_planHeadAngleKnotVector[j].first&&traj_start_time<last_planHeadAngleKnotVector[j+1].first)
					{	
						double time_ratio=(traj_start_time-last_planHeadAngleKnotVector[j].first)/(last_planHeadAngleKnotVector[j+1].first-last_planHeadAngleKnotVector[j].first);
						Eigen::VectorXd plan_start_head =(1-time_ratio)*last_planHeadAngleKnotVector[j].second+time_ratio*last_planHeadAngleKnotVector[j+1].second;
						planHeadAngleKnotVector[i].second=plan_start_head;
						break;
					}
				}
			}


		}
		else{
			double t_cur=sendBsplineTraj[0].duration_time/(planHeadAngleKnotVector.size()-1)*i;

			for(int id=0;id<AGENT_NUM;id++)
			{
				headcost_p[id]=sendBsplineTraj[id].evaluateDeBoorT(t_cur);
				Eigen::VectorXd velocity=sendBsplineTraj[id].getDerivative().evaluateDeBoorT(t_cur);
				velocity_angle(id)=std::atan2(velocity(1),velocity(0));
			}

			planHeadAngleKnotVector[i].first=traj_start_time+t_cur;


			for(int k=0;k<100;k++)IGSector[k][1]=-1;
			distributed_optimizer.initial_variable(planHeadAngleKnotVector[i-1].second);
			distributed_optimizer.round=i;
			last_head=planHeadAngleKnotVector[i-1].second;
			distributed_optimizer.PSO();
			// ROS_INFO("\033[1;%dm id: %d,head:%f %f %f %f, index:%d \033[0m",COLOR_ID,AGENT_ID,distributed_optimizer.variable(0),distributed_optimizer.variable(1),distributed_optimizer.variable(2),
			// distributed_optimizer.variable(3),i);

			planHeadAngleKnotVector[i].second=distributed_optimizer.variable;
			
		}
		
	}

}

void Formation_Agent::changeState(FSM_EXEC_STATE next_state)
{
    exec_state_=next_state;
	std::string state_string[7]={"INIT","WAIT_TARGET","GEN_NEW_TRAJ","REPLAN_TRAJ","EXEC_TRAJ","EMERGENCY_STOP","SEQUENTIAL_START"};
	ROS_INFO("\033[1;%dm id:%d Go to State: %s /////////////// \033[0m" ,COLOR_ID,AGENT_ID, state_string[exec_state_].c_str()   );

}

void Formation_Agent::init_path_visual(std::vector<Eigen::Vector3i> init_path)
{
	visualization_msgs::Marker line;

	for(int i=0;i<init_path.size();i++)
	{
		Eigen::Vector3d pos=gvmcb.getMapGlobalPosfromLocalIdx(init_path[i]);
		geometry_msgs::Point point;		
		point.x=pos(0);		point.y=pos(1);		point.z=pos(2);
		line.points.push_back(point);
	}

	line.id = 9109+AGENT_ID*2;
    line.header.frame_id = "map";
    line.header.stamp = ros::Time::now();
    line.type = visualization_msgs::Marker::LINE_STRIP;
    line.action = visualization_msgs::Marker::ADD;
    line.ns = "lines_and_points";
    line.scale.x = 0.1;
    line.scale.y = 0.1;
    line.scale.z = 0.1;
    line.lifetime = ros::Duration(0);

	std_msgs::ColorRGBA color;
	switch (AGENT_ID)
	{
		case 0:
			color.r=1.0;color.g=0;color.b=0.0;color.a=1;
		break;
		case 1:
			color.r=1.0;color.g=1;color.b=0.0;color.a=1;
		break;
		case 2:
			color.r=0.0;color.g=0;color.b=1.0;color.a=1;
		break;
		case 3:
			color.r=0.5;color.g=0.4;color.b=0.03;color.a=1;
		break;
	}
	line.color=color;

    init_path_visual_pub.publish(line);



}


void Formation_Agent::execFSMCallback(const ros::TimerEvent &e)
{
    if(!AgentInitFlag) return;
    static double time=ros::Time::now().toSec();

	// HeadPlanning();

	std::string state_string[7]={"INIT","WAIT_TARGET","GEN_NEW_TRAJ","REPLAN_TRAJ","EXEC_TRAJ","EMERGENCY_STOP","SEQUENTIAL_START"};

	// ROS_INFO_THROTTLE(1,"\033[1;%dm id:%d Current State: %s //////////////// \033[0m" ,COLOR_ID,AGENT_ID, state_string[exec_state_].c_str()   );

	// if(current_state.mode!="OFFBOARD" )   
	// {
	// 	changeState(INIT);
	// }   

    switch (exec_state_)
    {
    case INIT:
    {
        if((ros::Time::now().toSec()-time)>3.0/* &&current_state.mode=="OFFBOARD" */ )   
		{
			changeState(REPLAN_TRAJ);
		}   
		else
		{
			trajectory_msgs::JointTrajectory traj;

			trajectory_msgs::JointTrajectoryPoint point;
			point.positions.push_back(current_p[AGENT_ID](0));
			point.positions.push_back(current_p[AGENT_ID](1));
			point.positions.push_back(current_p[AGENT_ID](2));

			point.velocities.push_back(0);
			point.velocities.push_back(0);
			point.velocities.push_back(0);

			traj.points.push_back(point);
			traj.header.stamp=ros::Time::now();
			optimize_traj_pub.publish(traj);
		}
        break;
    }

    case EXEC_TRAJ:
    {
		if(execflag<0.5)
		{
			changeState(REPLAN_TRAJ);
			optimize_traj_visual_pub.publish(traj_points);
			break;
		}
		else{
			PathExchanger.send_Astar_path(a_star_pos_path,true);
			PathExchanger.sendtraj(sendBsplineTraj,trajcost);
			optimize_traj_visual_pub.publish(traj_points);
			// ROS_INFO_THROTTLE(0.5,"current_state EXEC_TRAJ local_goal: %f %f %f",local_goal(0),local_goal(1),local_goal(2));
			Eigen::Vector3d start_p=Eigen::Vector3d::Zero();
			for(int i=0;i<AGENT_NUM;i++)
			{
				start_p+=current_p[i];
			}
			start_p/=AGENT_NUM;
			// if((start_p-local_goal).norm()<replan_length)
			// {
			// 	changeState(REPLAN_TRAJ);
			// }

			if(sendBsplineTraj[0].duration_time+sendBsplineTraj[0].start_time-  ros::Time::now().toSec()<PlanningLeftTime)
			{
				changeState(REPLAN_TRAJ);
			}
			break;
		}
    }
    case REPLAN_TRAJ:
    {
		ros::Time plan_start=ros::Time::now();
		ROS_INFO("\033[1;%dm id:%d enter into REPLAN_TRAJ State \033[0m",COLOR_ID,AGENT_ID);
        Eigen::Vector3d start_p=Eigen::Vector3d::Zero();

		double plan_start_time=ros::Time::now().toSec()+PlanUsingTime;
		std::vector<Eigen::Vector3d> plan_start_p(AGENT_NUM);
		std::vector<Eigen::Vector3d> plan_start_v(AGENT_NUM);

		if(FirstPlan||PlanUsingCurrentState)
		{
			for(int id=0;id<AGENT_NUM;id++)
			{
				plan_start_p[id]= current_p[id];
				plan_start_v[id]=current_v[id];
				start_p+=plan_start_p[id];
			}
			start_p/=AGENT_NUM;			
		}

		else
		{
			for(int id=0;id<AGENT_NUM;id++)
			{
				double t_cur=plan_start_time-sendBsplineTraj[id].start_time;
				plan_start_p[id]= sendBsplineTraj[id].evaluateDeBoorT(t_cur);
				plan_start_v[id]=sendBsplineTraj[id].getDerivative().evaluateDeBoorT(t_cur);
				start_p+=plan_start_p[id];
			}
			start_p/=AGENT_NUM;
		}
		FirstPlan=false;


        double length=(global_goal-start_p).norm();

        if((current_p[AGENT_ID] -global_goal).norm()<=local_length)
        {
            plan_end_p=global_goal;
        }
        else
            plan_end_p=(1-local_length/length)*start_p+local_length/length*global_goal;

		sended_traj_start_time=plan_start_time;
		//find a path from formation centre to local end point centre
		a_star_path.clear();
        if(astar_searcher.a_star_search(start_p,global_goal,SAFETY_DISTANCE*1.5,local_goal,a_star_path))
        {
			a_star_pos_path.clear();
			for(int i=0;i<(int)a_star_path.size();i++)
			{	
				// ROS_INFO("id: %d POINT: %d %d %d",AGENT_ID,a_star_path[i](0),a_star_path[i](1),a_star_path[i](2));
				Eigen::Vector3d pos=gvmcb.getMapGlobalPosfromLocalIdx(a_star_path[i]);
				if(i==0)
					a_star_pos_path.push_back(start_p);
				else
					a_star_pos_path.push_back(pos);
			}
			PathExchangerLock=true;
			ros::Time start=ros::Time::now();
			// ROS_INFO("\033[1;%dm id: %d enter into exchange path ||||||||||||||| \033[0m",COLOR_ID,AGENT_ID);
			std::vector<Eigen::Matrix<double,6,1>> current_state(AGENT_NUM);
			for(int id=0;id<AGENT_NUM;id++)
			{
				current_state[id].head(3)=plan_start_p[id];
				current_state[id].tail(3)=plan_start_v[id];
			}
			PathExchanger.exchangeAstarPath(a_star_pos_path,current_state);
			for(int id=0;id<AGENT_NUM;id++)
			{
				plan_start_p[id]=current_state[id].head(3);
				plan_start_v[id]=current_state[id].tail(3);
				// ROS_INFO("id: %d after exchange: current_p[%d]: %f %f %f current_v: %f %f %f",
				// 	AGENT_ID,id,current_p[id](0),current_p[id](1),current_p[id](2),current_v[id](0),current_v[id](1),current_v[id](2));
			}
			

			local_goal=a_star_pos_path.back();
			PathExchangerLock=false;
			Eigen::Vector3d endpoint=a_star_pos_path.back();
			ros::Time end=ros::Time::now();
			ROS_INFO("\033[1;%dm id: %d Exchange path use: %f endpoint: %f %f %f |||||||||\033[0m",COLOR_ID,AGENT_ID,(end-start).toSec(),endpoint(0),endpoint(1),endpoint(2));
			a_star_path.clear();
			// std::cout<<"a star path:"<<std::endl;
			for(int i=0;i<(int)a_star_pos_path.size();i++)
			{	
				Eigen::Vector3i idx;
				gvmcb.getmapIdxLocal(a_star_pos_path[i],idx);
				a_star_path.push_back(idx);
				// if(AGENT_ID==0)
				// {
				// 	std::cout<<a_star_pos_path[i](0)<<" "<<a_star_pos_path[i](1)<<" "<<a_star_pos_path[i](2)<<std::endl;
				// }
			}	

			std::vector<Eigen::Vector3i> init_path=formation_trajectory_optimizer.SetInitPath(plan_start_p,local_goal, a_star_path );
			init_path_visual(init_path);
			std::vector<Eigen::Vector3d> tmp_plan_start_p(AGENT_NUM);
			for(int id=0;id<AGENT_NUM;id++)
			{
				sendBsplineTraj[id].start_time=sended_traj_start_time;
				tmp_plan_start_p[id]=plan_start_p[id];
			}
			srand((ros::Time::now().toSec())/(AGENT_ID+0.5));
			double exit_start=ros::Time::now().toSec();
			while(1)
			{	
				if(formation_trajectory_optimizer.trajectroyOptimize(tmp_plan_start_p,OptimizedTraj,plan_start_v,sendBsplineTraj,trajcost))
				{
					PathExchangerLock=true;
					if(execflag>0.5)
						PathExchanger.exchangeOptimizeTraj(sendBsplineTraj,trajcost);
					// ROS_INFO("id :%d, traj.duration: %f",AGENT_ID,sendBsplineTraj[0].duration_time);
					PathExchangerLock=false;
					break;
				}		

				PathExchangerLock=true;
				if(PathExchanger.queryTraj(sendBsplineTraj,trajcost))
				{
					formation_trajectory_optimizer.optimize_round++;
					break;
				}
				PathExchangerLock=false;

				Eigen::Vector3d random_offset(0,0,0);
				double max=0.2;
				double min=-0.2;
				random_offset(0)=(max-min)*rand()/(RAND_MAX)+min;
				random_offset(1)=(max-min)*rand()/(RAND_MAX)+min;

				for(int id=0;id<AGENT_NUM;id++)
				{
					if(id==AGENT_ID)continue;
					tmp_plan_start_p[id]=plan_start_p[id]+random_offset;
				}

				ros::Duration(0.005).sleep();
				if((ros::Time::now().toSec()-exit_start)>5.0)
					exit(0);
			}
			sended_traj_start_time=sendBsplineTraj[0].start_time;

			if(PlanningHeadFlag)
			{
				ros::Time distribute_start=ros::Time::now();
				DistributeOptimizingHeadAngle();
				ros::Time distribute_end=ros::Time::now();
				ROS_INFO("\033[1;%dm id: %d Distribute Optimize using time: %f ???????????",COLOR_ID,AGENT_ID,(distribute_end-distribute_start).toSec());
			}
			traj_pub();
			changeState(EXEC_TRAJ);

        }

		ros::Time plan_end=ros::Time::now();
		ROS_INFO("ID: %d, plan use time: %f",AGENT_ID,(plan_end-plan_start).toSec());
        break;
    }


    }




}


